"""
荷载控制

date: 2025/8/16
author: SiHeng Tang
file: payload.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging
from copy import deepcopy

from async_sche import TaskWrapper, BaseTaskGen
from mengji import Vehicle
from mj_errors import MJRuntimeError
from mod_loader import ConfigHandler

logger = logging.getLogger(__name__)
handler = ConfigHandler(__name__)

DEFAULT_CONFIG = {
    "label": "payload",
    "plane_tele_label": "plane_tele",
    "payload_ctl_servo_idx": 7,
    "payload_ctl_servo_lock_pwm": 1100,
    "payload_ctl_servo_unlock_pwm": 1900,
    "dry_run": 1
}


class PayloadManage(BaseTaskGen):
    """
    荷载管理，抽象整个投放序列
    """

    def __init__(self, veh: Vehicle, cfg: dict):
        super().__init__(priority=TaskWrapper.IDLE_PRIORITY + 10)
        self._veh = veh
        self._config = cfg
        self._is_dry_run = self._config["dry_run"]

        self._payload_released = False

        if self._is_dry_run:
            logger.warning("payload manage is dry running, will not release payload!")

    def _on_exit(self):
        self._veh = None

    ########################
    # 访问接口
    ########################
    def has_activated(self):
        """
        查询是否启动过投放序列
        :return: 启动过 True 反之 False
        """
        return self._payload_released

    def activate_release_sequence(self):
        """
        激活投放序列
        :return: 序列激活成功返回 1 失败返回 0
        """
        logger.warning("Try active payload release sequence...")

        if self._config["dry_run"]:
            logger.warning("Dry run mode, not send command to controller.")
            return 1

        if self._payload_released:
            logger.warning("Repeated called payload release, ignore this request.")
            return 0

        try:
            plane_tele_node = self._veh.get_a(self._config["plane_tele_label"])
        except MJRuntimeError:
            return 0
        else:
            # MAV_CMD_DO_SET_SERVO (183)
            plane_tele_node.send_long_message(183,
                                              param1=self._config["payload_ctl_servo_idx"],
                                              param2=self._config["payload_ctl_servo_unlock_pwm"])
            logger.warning("Active payload release sequence!")
            self._payload_released = True
            return 1

    def command_PAYLOAD_SET(self, rpc_params: int):
        """
        荷载手动设置方法
        :param rpc_params: 整数，正数解锁载荷，负数锁定载荷
        :return:
        """
        if type(rpc_params) != int:
            return -1

        try:
            plane_tele_node = self._veh.get_a(self._config["plane_tele_label"])

        except MJRuntimeError:
            return -1

        else:
            # MAV_CMD_DO_SET_SERVO (183)
            if rpc_params:
                plane_tele_node.send_long_message(183,
                                                  param1=self._config["payload_ctl_servo_idx"],
                                                  param2=self._config["payload_ctl_servo_unlock_pwm"])
            else:
                plane_tele_node.send_long_message(183,
                                                  param1=self._config["payload_ctl_servo_idx"],
                                                  param2=self._config["payload_ctl_servo_lock_pwm"])

            return 0


@handler.add_handler("virtual_bus:payload")
def load_config(veh: Vehicle, cfg_section: dict):
    logger.debug(f"payload manager load config {cfg_section}")

    cfg = deepcopy(DEFAULT_CONFIG)
    cfg.update(cfg_section)

    veh.reg_part(cfg["label"], node := PayloadManage(veh, cfg), is_sche=True)
    veh.get_a("api").add_command("payload/set", node.command_PAYLOAD_SET)
